import numpy as np
from adrc_fhan import fhan
from zhnplot import SinglePlot

def plot_fhan(a, h=1, isplot=True):
    x_1 = x_2 = 0
    for r_ in a:
        x1_p = x_1 - h * x_2 + h * h * r_
        x2_p = x_2 - h * r_
        if isplot:
            fig1.plot([x_1, x1_p], [x_2, x2_p], color='black', linewidth=1)
        x_1, x_2 = x1_p, x2_p
    return x_1, x_2

R = 1
H = 1
L = 12
MaxSpeed = 4
r = R * np.ones(L)
fig1 = SinglePlot(labels=[r'$x_1$', r'$x_2$'], ticks=[[-75, -50, -25, 0, 25, 50, 75], []])
fig1.plot([-0.5*(L+1)**2, 0.5*(L+1)**2], [MaxSpeed, MaxSpeed], color='black', linewidth=1, linestyle='--')
fig1.plot([-0.5*(L+1)**2, 0.5*(L+1)**2], [-MaxSpeed, -MaxSpeed], color='black', linewidth=1, linestyle='--')
plot_fhan(r)
plot_fhan(-r)

r[0] = -R
x1 = x2 = 0
for N in range(2, L+1):
    r = R * np.ones(N)
    r[-1] = -R
    x1p, x2p = plot_fhan(r, isplot=False)
    if N == 2:
        fig1.plot([-H * H * R, x1p], [H * R, x2p], color='black', linewidth=1)
    else:
        fig1.plot([x1, x1p], [x2, x2p], color='black', linewidth=1)
    x1, x2 = x1p, x2p
x1 = x2 = 0
for N in range(2, L+1):
    r = -R * np.ones(N)
    r[-1] = R
    x1p, x2p = plot_fhan(r, isplot=False)
    if N == 2:
        fig1.plot([H * H * R, x1p], [-H * R, x2p], color='black', linewidth=1)
    else:
        fig1.plot([x1, x1p], [x2, x2p], color='black', linewidth=1)
    x1, x2 = x1p, x2p

x1p, x2p = x1, x2 = 10, 7
while True:
    u = fhan(x1, x2, R, H, MaxSpeed)
    x1 += H * x2
    x2 += H * u
    fig1.plot([x1, x1p], [x2, x2p], color='black', linewidth=1)
    x1p, x2p = x1, x2
    if -1e-4 < x1 < 1e-4 and -1e-4 < x2 < 1e-4 and -1e-4 < u < 1e-4:
        break

fig1.show()
print('finished1.')
